import json
import os
import math
import time

class Vector3:
    def __init__(self, x=0.0, y=0.0, z=0.0):
        self.x = x
        self.y = y
        self.z = z
    
    def __repr__(self):
        return f"Vector3({self.x}, {self.y}, {self.z})"
    
    def magnitude(self):
        return math.sqrt(self.x**2 + self.y**2 + self.z**2)
    
    def normalized(self):
        mag = self.magnitude()
        if mag < 0.001:
            return Vector3()
        return Vector3(self.x/mag, self.y/mag, self.z/mag)
    
    def __sub__(self, other):
        return Vector3(self.x - other.x, self.y - other.y, self.z - other.z)
    
    def __add__(self, other):
        return Vector3(self.x + other.x, self.y + other.y, self.z + other.z)
    
    def __mul__(self, scalar):
        return Vector3(self.x * scalar, self.y * scalar, self.z * scalar)

class ScannerData:
    def __init__(self, json_data):
        # 解析JSON数据到对象属性
        self.repulsionCoefficient = json_data.get('repulsionCoefficient', 2.0)
        self.entropyCoefficient = json_data.get('entropyCoefficient', 3.0)
        self.distanceCoefficient = json_data.get('distanceCoefficient', 2.0)
        self.leaderRangeCoefficient = json_data.get('leaderRangeCoefficient', 3.0)
        self.directionRetentionCoefficient = json_data.get('directionRetentionCoefficient', 2.0)
        self.updateInterval = json_data.get('updateInterval', 0.2)
        
        self.moveSpeed = json_data.get('moveSpeed', 2.0)
        self.rotationSpeed = json_data.get('rotationSpeed', 120.0)
        self.scanRadius = json_data.get('scanRadius', 5.0)
        
        self.maxRepulsionDistance = json_data.get('maxRepulsionDistance', 5.0)
        self.minSafeDistance = json_data.get('minSafeDistance', 2.0)
        
        self.avoidRevisits = json_data.get('avoidRevisits', True)
        self.targetSearchRange = json_data.get('targetSearchRange', 20.0)
        self.revisitCooldown = json_data.get('revisitCooldown', 60.0)
        
        # 解析Vector3类型的数据
        pos_data = json_data.get('position', {})
        self.position = Vector3(pos_data.get('x', 0), pos_data.get('y', 0), pos_data.get('z', 0))
        
        forward_data = json_data.get('forward', {})
        self.forward = Vector3(forward_data.get('x', 0), forward_data.get('y', 0), forward_data.get('z', 1))
        
        score_dir_data = json_data.get('scoreDir', {})
        self.scoreDir = Vector3(score_dir_data.get('x', 0), score_dir_data.get('y', 0), score_dir_data.get('z', 0))
        
        collide_dir_data = json_data.get('collideDir', {})
        self.collideDir = Vector3(collide_dir_data.get('x', 0), collide_dir_data.get('y', 0), collide_dir_data.get('z', 0))
        
        path_dir_data = json_data.get('pathDir', {})
        self.pathDir = Vector3(path_dir_data.get('x', 0), path_dir_data.get('y', 0), path_dir_data.get('z', 0))
        
        leader_range_dir_data = json_data.get('leaderRangeDir', {})
        self.leaderRangeDir = Vector3(leader_range_dir_data.get('x', 0), leader_range_dir_data.get('y', 0), leader_range_dir_data.get('z', 0))
        
        direction_retention_dir_data = json_data.get('directionRetentionDir', {})
        self.directionRetentionDir = Vector3(direction_retention_dir_data.get('x', 0), direction_retention_dir_data.get('y', 0), direction_retention_dir_data.get('z', 0))
        
        final_move_dir_data = json_data.get('finalMoveDir', {})
        self.finalMoveDir = Vector3(final_move_dir_data.get('x', 0), final_move_dir_data.get('y', 0), final_move_dir_data.get('z', 0))
        
        # 解析Leader信息
        leader_pos_data = json_data.get('leaderPosition', {})
        self.leaderPosition = Vector3(leader_pos_data.get('x', 0), leader_pos_data.get('y', 0), leader_pos_data.get('z', 0))
        self.leaderScanRadius = json_data.get('leaderScanRadius', 0.0)
        
        # 解析已访问蜂窝记录
        self.visitedCells = []
        for cell_data in json_data.get('visitedCells', []):
            self.visitedCells.append(Vector3(cell_data.get('x', 0), cell_data.get('y', 0), cell_data.get('z', 0)))
    
    def to_dict(self):
        # 将对象转换回字典格式以便序列化
        result = {
            'repulsionCoefficient': self.repulsionCoefficient,
            'entropyCoefficient': self.entropyCoefficient,
            'distanceCoefficient': self.distanceCoefficient,
            'leaderRangeCoefficient': self.leaderRangeCoefficient,
            'directionRetentionCoefficient': self.directionRetentionCoefficient,
            'updateInterval': self.updateInterval,
            
            'moveSpeed': self.moveSpeed,
            'rotationSpeed': self.rotationSpeed,
            'scanRadius': self.scanRadius,
            
            'maxRepulsionDistance': self.maxRepulsionDistance,
            'minSafeDistance': self.minSafeDistance,
            
            'avoidRevisits': self.avoidRevisits,
            'targetSearchRange': self.targetSearchRange,
            'revisitCooldown': self.revisitCooldown,
            
            'position': {'x': self.position.x, 'y': self.position.y, 'z': self.position.z},
            'forward': {'x': self.forward.x, 'y': self.forward.y, 'z': self.forward.z},
            'scoreDir': {'x': self.scoreDir.x, 'y': self.scoreDir.y, 'z': self.scoreDir.z},
            'collideDir': {'x': self.collideDir.x, 'y': self.collideDir.y, 'z': self.collideDir.z},
            'pathDir': {'x': self.pathDir.x, 'y': self.pathDir.y, 'z': self.pathDir.z},
            'leaderRangeDir': {'x': self.leaderRangeDir.x, 'y': self.leaderRangeDir.y, 'z': self.leaderRangeDir.z},
            'directionRetentionDir': {'x': self.directionRetentionDir.x, 'y': self.directionRetentionDir.y, 'z': self.directionRetentionDir.z},
            'finalMoveDir': {'x': self.finalMoveDir.x, 'y': self.finalMoveDir.y, 'z': self.finalMoveDir.z},
            
            'leaderPosition': {'x': self.leaderPosition.x, 'y': self.leaderPosition.y, 'z': self.leaderPosition.z},
            'leaderScanRadius': self.leaderScanRadius,
            
            'visitedCells': [{'x': cell.x, 'y': cell.y, 'z': cell.z} for cell in self.visitedCells]
        }
        return result

class ScannerAlgorithm:
    def __init__(self):
        # 初始化算法参数
        pass
    
    def calculate_weights(self, data):
        # 计算各权重
        total = (data.repulsionCoefficient + data.entropyCoefficient + data.distanceCoefficient + 
                data.leaderRangeCoefficient + data.directionRetentionCoefficient)
        
        # 处理所有系数都为0的特殊情况
        if total < 0.001:
            return 0.2, 0.2, 0.2, 0.2, 0.2
        
        repulsion_weight = data.repulsionCoefficient / total
        entropy_weight = data.entropyCoefficient / total
        distance_weight = data.distanceCoefficient / total
        leader_range_weight = data.leaderRangeCoefficient / total
        direction_retention_weight = data.directionRetentionCoefficient / total
        
        return repulsion_weight, entropy_weight, distance_weight, leader_range_weight, direction_retention_weight
    
    def calculate_repulsion_ratio(self, distance, min_safe_distance, max_repulsion_distance):
        # 计算排斥力比例
        if distance <= min_safe_distance:
            return 1.0
        if distance >= max_repulsion_distance:
            return 0.0
        
        # 非线性衰减，近距离排斥力增长更快
        t = (distance - min_safe_distance) / (max_repulsion_distance - min_safe_distance)
        return 1.0 - (t * t)
    
    def process(self, data):
        """处理扫描器数据并计算新的移动方向"""
        # 这里是算法的主要逻辑，根据需要替换为更复杂的实现
        
        # 计算权重
        repulsion_weight, entropy_weight, distance_weight, leader_range_weight, direction_retention_weight = self.calculate_weights(data)
        
        # 在实际应用中，这里应该根据环境数据和算法计算新的方向向量
        # 这里为了示例，我们简单地复用Unity发送的数据并稍微调整
        
        # 假设我们已经计算了新的各方向向量
        # 实际应用中，这里应该是复杂的算法实现
        new_score_dir = data.scoreDir
        new_path_dir = data.pathDir
        new_collide_dir = data.collideDir
        new_leader_range_dir = data.leaderRangeDir
        new_direction_retention_dir = data.directionRetentionDir
        
        # 合并所有向量
        new_final_move_dir = (
            new_score_dir * entropy_weight + 
            new_path_dir * distance_weight + 
            new_collide_dir * repulsion_weight +
            new_leader_range_dir * leader_range_weight +
            new_direction_retention_dir * direction_retention_weight
        )
        
        # 归一化最终方向
        if new_final_move_dir.magnitude() > 0.1:
            new_final_move_dir = new_final_move_dir.normalized()
        else:
            # 如果最终方向接近零，保持当前方向
            new_final_move_dir = data.forward
        
        # 更新数据对象中的方向向量
        data.finalMoveDir = new_final_move_dir
        
        return data

def main():
    # 输入输出文件路径
    input_file = "python_input.json"
    output_file = "python_output.json"
    
    print(f"扫描器算法处理程序启动")
    print(f"输入文件: {input_file}")
    print(f"输出文件: {output_file}")
    
    # 创建算法实例
    algorithm = ScannerAlgorithm()
    
    try:
        # 读取Unity发送的数据
        with open(input_file, 'r', encoding='utf-8') as f:
            json_data = json.load(f)
            
        # 解析数据
        scanner_data = ScannerData(json_data)
        print(f"成功读取数据，位置: {scanner_data.position}")
        
        # 处理数据
        processed_data = algorithm.process(scanner_data)
        
        # 将处理后的数据转换为字典并保存到文件
        result_dict = processed_data.to_dict()
        with open(output_file, 'w', encoding='utf-8') as f:
            json.dump(result_dict, f, indent=2, ensure_ascii=False)
        
        print(f"处理完成，结果已保存到 {output_file}")
        
    except Exception as e:
        print(f"处理过程中发生错误: {str(e)}")

if __name__ == "__main__":
    main()